
// F. Chaxel 2010

// Singleton and Observer design patterns are used here

#ifndef ROBOT_H
#define ROBOT_H

#include "Motors.h"

#include "ThreadObject.h"

#include "IRobotAdvise.h"

#include <iostream>
#include <math.h>
#include <pthread.h>
#include <unistd.h>

// Size max of actions' PIPO
#define NB_MAXACTIONS 10

// in mm
#define WHEEL_DIAMETER 96L
#define WHEEL_DISTANCE 236L
#define ENCODER_COUNT_SHAFT 360L

// general constants
#define PI 3.1415
#define CIRCLE_DEGREES 360.0

// exeptable error, in encoder count difference
#define EPSILON 20

// TimeOut Command
#define TIMEOUT_COMM 40


class Robot;

typedef void (Robot::*returnFct)(long, long);

struct actions
{
	unsigned char type;
	long	value;
};

enum ActionListed {
	Null=0,
	TurnRight=1, 
	TurnLeft=2,
	TurnOnPlace=3, 
	GoForward=4
};

struct pos
{
	double x;
	double y;
	double radian;
};

class Robot:public ThreadObject
{	
	private :
		long TotalMovePosition;
		static Robot* _instance;

		Motors motors;

		long	Wheel_Right_Objective,Wheel_Left_Objective;
		long	Wheel_Right_Position,Wheel_Left_Position;
		char	RightMovePlus, LeftMovePlus;

		bool	in_progress;
		bool	kill_robot;
		bool	do_emergency_stop;
		pthread_spinlock_t lockin_progress;

		int ActionsListPtrLow,ActionsListPtrHigh;
		struct actions ActionsList[NB_MAXACTIONS];

		returnFct SetPosition;
		long TotalLengthPosition;

		IRobotAdvise *Advised_Object;	// only one observer here

		struct pos Position;

	private:		
		bool Concat(unsigned char actiontype, long value);
		bool Scheduler(unsigned char actiosntype, long value);
		static void *RobotAdviseThread( void *ptrAdvise );
		double TurnHelper(long degree);
		void SavePostion(unsigned char actiontype, int value);
		void Run();	
	
	protected:
		virtual unsigned char CalcWheelSpeed(long diff,char MoveDirection);
		returnFct SavePosition(unsigned char, long,long,long);
		
		void TurnRightPosition(long delta_left,long delta_right);
		void TurnLeftPosition(long delta_left,long relatif_right);
		void GoForwardPosition(long delta_left,long delta_right);
		void TurnOnPlacePosition(long delta_left,long delta_right);

	public:
		Robot();
		virtual ~Robot();
		float Battery_Voltage();
		double Get_Position(char Coorconne);
		bool TurnLeft(long degree);
		bool TurnRight(long degree);
		bool TurnOnPlace(long degree);
		bool GoForward(long GoForward);
		bool Is_Stopped();
		bool Is_Killed();


		void EmergencyStop();
		void Kill_Robot();
		void Advise(IRobotAdvise *Listener);
		void UnAdvise(IRobotAdvise *Listener);

};

#endif
